
#include <BCube>
#include <BCone>
#include <BCylinder>
#include <BCapsule>
#include <BTorus>
#include <BSphere>
#include <BTube>

#include "IOverlapPair.h"
#include "IAlgorithm.h"
#include "IConvexConcaveAlgorithm.h"
#include "IVoronoiSimplexSolver.h"
#include "ISimplexConvexCast.h"
#include "member_BShape.h"
#include "member_BMesh.h"
#include "member_BGeometry.h"
#include "member_BSurface.h"
#include "member_BTerrain.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

IConvexConcaveAlgorithm::IConvexConcaveAlgorithm()
{

}
IConvexConcaveAlgorithm::~IConvexConcaveAlgorithm()
{

}

void IConvexConcaveAlgorithm::caculate()
{
	return;
	if (!dynamic_cast<BCube*>(_shapeA) &&
		!dynamic_cast<BCone*>(_shapeA) &&
		!dynamic_cast<BCylinder*>(_shapeA) &&
		!dynamic_cast<BCapsule*>(_shapeA) &&
		!dynamic_cast<BTorus*>(_shapeA) &&
		!dynamic_cast<BSphere*>(_shapeA) &&
		!dynamic_cast<BTube*>(_shapeA))
	{
		this->swap();
	}
	if (BMesh* mesh = dynamic_cast<BMesh*>(_shapeB))
	{
		_callback.setup(mesh->margin(), _bodyA, _shapeA, _matrixA, _bodyB, _shapeB, _matrixB);
		BMatrix matrix = _matrixB.inverse() * _matrixA;
		BSpace space = shape_member(_shapeA)->transSpace(matrix);
		space.min() -= mesh->margin();
		space.max() += mesh->margin();
		mesh_member(mesh)->overlapping(&_callback, space);
		this->refreshContactPoints();
		return;
	}
	if (BGeometry* geometry = dynamic_cast<BGeometry*>(_shapeB))
	{
		_callback.setup(geometry->margin(), _bodyA, _shapeA, _matrixA, _bodyB, _shapeB, _matrixB);
		BMatrix matrix = _matrixB.inverse() * _matrixA;
		BSpace space = shape_member(_shapeA)->transSpace(matrix);
		space.min() -= geometry->margin();
		space.max() += geometry->margin();
		geometry_member(geometry)->overlapping(&_callback, space);
		this->refreshContactPoints();
		return;
	}
	if (BSurface* surface = dynamic_cast<BSurface*>(_shapeB))
	{
		_callback.setup(surface->margin(), _bodyA, _shapeA, _matrixA, _bodyB, _shapeB, _matrixB);
		BMatrix matrix = _matrixB.inverse() * _matrixA;
		BSpace space = shape_member(_shapeA)->transSpace(matrix);
		space.min() -= surface->margin();
		space.max() += surface->margin();
		surface_member(surface)->processTriangles(&_callback, space);
		this->refreshContactPoints();
		return;
	}
	if (BTerrain* terrain = dynamic_cast<BTerrain*>(_shapeB))
	{
		_callback.setup(terrain->margin(), _bodyA, _shapeA, _matrixA, _bodyB, _shapeB, _matrixB);
		BMatrix matrix = _matrixB.inverse() * _matrixA;
		BSpace space = shape_member(_shapeA)->transSpace(matrix);
		space.min() -= terrain->margin();
		space.max() += terrain->margin();
		terrain_member(terrain)->overlapping(&_callback, space);
		this->refreshContactPoints();
		return;
	}
}

void IConvexConcaveAlgorithm::timeImpact()
{
	BBody* convexbody = _bodyA;
	BBody* triBody = _bodyB;

	const BMatrix& matrixA = node_member(convexbody)->matrix;
	const BMatrix& matrixB = node_member(triBody)->matrix;

	const BMatrix& interpolationWorldTransformA = body_member(convexbody)->predictedMatrix;
	const BMatrix& interpolationWorldTransformB = body_member(triBody)->predictedMatrix;

	BMatrix triInv = matrixB.inverse();
	BMatrix convexFromLocal = triInv * matrixA;
	BMatrix convexToLocal = triInv * interpolationWorldTransformA;

	struct LocalTriangleSphereCastCallback
	{
		BMesh* m_mesh;
		BMatrix m_ccdSphereFromTrans;
		BMatrix m_ccdSphereToTrans;
		BMatrix m_meshTransform;

		BReal m_ccdSphereRadius;
		BReal m_hitFraction;

		LocalTriangleSphereCastCallback(BMesh* mesh, const BMatrix& from, const BMatrix& to, BReal ccdSphereRadius, BReal hitFraction)
		{
			m_mesh = mesh;
			m_ccdSphereFromTrans = from;
			m_ccdSphereToTrans = to;
			m_ccdSphereRadius = ccdSphereRadius;
			m_hitFraction = hitFraction;
		}

		virtual void processTriangle(const BSpace& space)
		{
			const BVectorArray* vertices = m_mesh->vertices();
			const BFaceArray* faces = m_mesh->faces();
			for (int i = 0; i < faces->size(); i++)
			{
				const BFace& face = faces->at(i);
				const BVector& va = vertices->at(face.a());
				const BVector& vb = vertices->at(face.b());
				const BVector& vc = vertices->at(face.c());

				CastResult caseResult;
				caseResult.fraction = m_hitFraction;
				BSphere pointShape(m_ccdSphereRadius);
				ITriangleShape triShape(va, vb, vc);
				IVoronoiSimplexSolver simplexSolver;
				ISimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);

				BMatrix ident;
				convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, ident, ident, caseResult);
				if (m_hitFraction > caseResult.fraction)
					m_hitFraction = caseResult.fraction;
			}
		}
	};

	if (BMesh* mesh = dynamic_cast<BMesh*>(_shapeB))
	{
		BVector rayAabbMin = convexFromLocal.position();
		SetMin(rayAabbMin, convexToLocal.position());
		BVector rayAabbMax = convexFromLocal.position();
		SetMax(rayAabbMax, convexToLocal.position());
		BReal ccdRadius0 = convexbody->box().radius();
		rayAabbMin -= BVector(ccdRadius0, ccdRadius0, ccdRadius0);
		rayAabbMax += BVector(ccdRadius0, ccdRadius0, ccdRadius0);

		BReal curHitFraction = 1;  //is this available?
		LocalTriangleSphereCastCallback raycastCallback(mesh, convexFromLocal, convexToLocal, ccdRadius0, curHitFraction);

		raycastCallback.m_hitFraction = body_member(convexbody)->solverData.hitFraction;

		BBody* concavebody = triBody;
		BMesh* triangleMesh = (BMesh*)_shapeB;
		if (triangleMesh)
		{
			raycastCallback.processTriangle(BSpace(rayAabbMin, rayAabbMax));
		}

		if (body_member(convexbody)->solverData.hitFraction > raycastCallback.m_hitFraction)
		{
			body_member(convexbody)->solverData.hitFraction = raycastCallback.m_hitFraction;
		}
	}

}
